#include <FlexiTimer2.h>
#include <Arduino.h>

#define S0    A4   //物体表面的反射光越强，TCS3002D内置振荡器产生的方波频率越高，
#define S1    A5  //S0和S1的组合决定输出信号频率比例因子，比例因子为2%
#define S2     8   //S2和S3的组合决定让红、绿、蓝，哪种光线通过滤波器
#define S3     3
#define OUT    2  //TCS3200颜色传感器输出信号连接到Arduino中断0引脚，并引发脉冲信号中断
#define LED    A3  //控制TCS3200颜色传感器是否点亮LED灯

int color_detection_ballon = 0;
int red = 0; 
int green = 0; 
int blue = 0; 


void Color_Init()
{
  pinMode(S0, OUTPUT);  
  pinMode(S1, OUTPUT);  
  pinMode(S2, OUTPUT);  
  pinMode(S3, OUTPUT);  
  pinMode(OUT, INPUT);  
  pinMode(LED, OUTPUT);
  digitalWrite(S0, HIGH);   
  digitalWrite(S1, HIGH);
}

int return_color_ballon()
{
  digitalWrite(LED, HIGH);
  int numbers_count = 0;
  int color_judge[12]={0,0,0,0,0,0,0,0,0,0,0,0};

  Serial.println("---------------Start---------------");
  unsigned long time_now = millis();
  while( (millis() - time_now ) < 2000)
  {
     numbers_count ++;
     digitalWrite(S2, LOW); 
     digitalWrite(S3, LOW); 
     //count OUT, pRed, RED 
     red = pulseIn(OUT, digitalRead(OUT) == HIGH ? LOW : HIGH); //blue
     digitalWrite(S3, HIGH); 
     //count OUT, pBLUE, BLUE 
     blue = pulseIn(OUT, digitalRead(OUT) == HIGH ? LOW : HIGH); //red
     digitalWrite(S2, HIGH); 
     //count OUT, pGreen, GREEN 
     green = pulseIn(OUT, digitalRead(OUT) == HIGH ? LOW : HIGH); //green
     if(red<blue){color_judge[0] = color_judge[0] +1;}  else{color_judge[1] = color_judge[1] +1;}
     if(red<green){color_judge[2] = color_judge[2] +1;} else{color_judge[3] = color_judge[3] +1;}

     if(blue<red){color_judge[4] = color_judge[4] +1;}  else{color_judge[5] = color_judge[5] +1;}
     if(blue<green){color_judge[6] = color_judge[6] +1;} else{color_judge[7] = color_judge[7] +1;}

     if(green<red){color_judge[8] = color_judge[8] +1;}  else{color_judge[9] = color_judge[9] +1;}
     if(green<blue){color_judge[10] = color_judge[10] +1;} else{color_judge[11] = color_judge[11] +1;}     
  }
  Serial.println();
  if( (color_judge[0] > color_judge[1])  && ((color_judge[2] > color_judge[3])) )
  {
#ifdef DEBUG
    Serial.println("The color is red");
#endif
    color_detection_ballon = 1;
    return 1;
  }

  else if( (color_judge[4] > color_judge[5])  && ((color_judge[6] > color_judge[7]))  )
  {
#ifdef DEBUG
    Serial.println("The color is blue");
#endif
    color_detection_ballon = 2;
  }

  else if( (color_judge[8] > color_judge[9])  && ((color_judge[10] > color_judge[11])) )
  {
#ifdef DEBUG
    Serial.println("The color is green");
#endif
    color_detection_ballon = 3;   
  }
  else
  {
#ifdef DEBUG
    Serial.println("None color");
#endif    
  }
  digitalWrite(LED, LOW);delay(500);
}
